
module cfo_top(
    input clk,
    input rst,
    input strb,
    input [15:0] in_r,
    input [15:0] in_i,
    output find_sp,
    output [15:0] cfo
    );
   parameter size = 128, width=16 ;
   parameter idle = 32'd0;
   parameter s0 =  32'h0000_0001,s1= 32'h0000_0002,s2= 32'h0000_0004,s3= 32'h0000_0008,s4= 32'h0000_0010;
   parameter s5 =  32'h0000_0020,s6= 32'h0000_0040,s7= 32'h0000_0080,s8= 32'h0000_0100,s9= 32'h0000_0200;
   parameter s10 = 32'h0000_0400,s11=32'h0000_0800,s12=32'h0000_1000,s13=32'h0000_2000,s14=32'h0000_4000;
   parameter s15 = 32'h0000_8000,s16=32'h0001_0000,s17=32'h0002_0000,s18=32'h0004_0000,s19=32'h0008_0000;
   parameter s20 = 32'h0010_0000,s21=32'h0020_0000,s22=32'h0040_0000,s23=32'h0080_0000,s24=32'h0100_0000;
   parameter s25 = 32'h0200_0000,s26=32'h0400_0000,s27=32'h0800_0000,s28=32'h1000_0000,s29=32'h2000_0000;
   parameter s30 = 32'h4000_0000,s31=32'h8000_0000;
   reg [31:0] state;
   
   //data load s0
   reg [width-1:0] seq_r [0:size-1];
   reg [width-1:0] seq_i [0:size-1];
   integer i;
   always @(posedge clk) begin
        if(rst) begin
            seq_r[0]<='b0;
            seq_i[0]<='b0;
            seq_r[size-1]<='b0;
            seq_i[size-1]<='b0;

        end
        else case(state)
            s0:begin
                seq_r[0]<=in_r;
                seq_i[0]<=in_i;
                for(i=size-1;i>0;i=i-1) begin
                    seq_r[i]<=seq_r[i-1];
                    seq_i[i]<=seq_i[i-1];
                end
            end
        endcase
   end
   
   //mul s0-s6
   reg [width-1:0] last_r;
   reg [width-1:0] last_i;
   reg mul_start_r;
   wire mul_start;
   assign mul_start = mul_start_r;
   always @(posedge clk) begin
        if(rst) begin
            last_r<='b0;
            last_i<='b0;
            mul_start_r<=1'b0;
        end
        else case(state)
            s0:begin
                last_r<=seq_r[size-1];
                last_i<=seq_i[size-1];
                mul_start_r<=1'b1;
            end
        endcase
   end 
   
   wire [width-1:0] seq_l_r;
   wire [width-1:0] seq_l_i;
   wire [width-1:0] seq_f_r;
   wire [width-1:0] seq_f_i;
   assign seq_l_r = last_r;
   assign seq_l_i = last_i;
   assign seq_f_r = seq_r[0];
   assign seq_f_i = seq_i[0];
   
   parameter m_width =32;
   wire [m_width-1:0] z_r;
   wire [m_width-1:0] z_i;
   
   wire [m_width-1:0] p_r;      //power

   /*
   mul_conj mul_ins(clk,mul_start,
           seq_f_r,seq_f_i,
           seq_l_r,seq_l_i,
           z_r,z_i,
       );
       */
    mul_conj mul_ins(clk,mul_start,
        seq_l_r,seq_l_i,
        seq_f_r,seq_f_i,
        z_r,z_i,
     );
     mul_conj mul_ins2(clk,mul_start,
        seq_f_r,seq_f_i,
        seq_f_r,seq_f_i,
        p_r
     );
   
   //read s7
   reg [m_width-1:0] z_r_r;     //cor
   reg [m_width-1:0] z_i_r;
   reg [m_width-1:0] p_r_r;     //pow

   always @(posedge clk) begin
        if(rst) begin
            z_r_r <= 'b0;
            z_i_r <= 'b0;
        end
        else case(state)
            s7:begin
                z_r_r <= z_r;
                z_i_r <= z_i;
            end 
        endcase
   end

   always @(posedge clk) begin
        if(rst) begin
            p_r_r <= 'b0;
        end
        else case(state)
            s7:begin
                p_r_r <= p_r;
            end 
        endcase
   end

   //xcorr seq
   parameter addsize = 9;
   reg [m_width-1:0] add_seq_r[0:addsize];
   reg [m_width-1:0] add_seq_i[0:addsize];
   integer i2;
   always @(posedge clk) begin
        if(rst) begin
            add_seq_r [1]         <= 'b0;
            add_seq_i [1]         <= 'b0;
            add_seq_r [addsize-1] <= 'b0;
            add_seq_i [addsize-1] <= 'b0;
        end
        else case(state)
            s8:begin
                for(i2=addsize;i2>0;i2=i2-1) begin
                    add_seq_r [i2] <= add_seq_r[i2-1];
                    add_seq_i [i2] <= add_seq_i[i2-1];
                end
                add_seq_r[0]     <= z_r_r;
                add_seq_i[0]     <= z_i_r;
            end
        endcase
   end

   //power seq
   reg [m_width-1:0] pow_seq_r[0:addsize];
   integer i3;
   always @(posedge clk) begin
        if(rst) begin
            pow_seq_r [1]           <= 'b0;
            pow_seq_r [addsize-1]   <= 'b0;
        end
        else case(state)
            s8:begin
                for(i3=addsize;i3>0;i3=i3-1) begin
                    pow_seq_r [i3] <= pow_seq_r[i3-1];

                end
                pow_seq_r [0]       <= p_r_r;
            end
        endcase
   end
   
   //sum s9
   parameter s_width = 36;
   reg signed [s_width-1:0] sum_r;
   reg signed [s_width-1:0] sum_i;
   reg signed [s_width-1:0] pow;
   always @(posedge clk) begin
        if(rst) begin
            sum_r<='b0;
            sum_i<='b0;
        end
        else case(state)
            s9:begin
                sum_r<= sum_r + $signed(add_seq_r[1])-$signed(add_seq_r[addsize-1]);
                sum_i<= sum_i + $signed(add_seq_i[1])-$signed(add_seq_i[addsize-1]);
                pow  <= pow   + $signed(pow_seq_r[1])-$signed(pow_seq_r[addsize-1]);
            end        
        endcase
   end
   
   //abs s10 s11
   reg [s_width-1:0] cor_re;
   reg [s_width-1:0] cor_im;
   reg [s_width:0] cor_r;
   always @(posedge clk) begin
        if(rst) begin
            cor_re <= 'b0;
            cor_im <= 'b0;    
            cor_r  <= 'b0;
        end
        else case(state)
            s10:begin
                cor_re <= sum_r [s_width-1]? ($unsigned(~sum_r)+1'b1) : sum_r;
                cor_im <= sum_i [s_width-1]? ($unsigned(~sum_i)+1'b1) : sum_i;
            end
            s11:begin
                cor_r  <= {1'b0, cor_re} + {1'b0,cor_im};
            end
        endcase
   end
   
   //compare number s10
   reg [s_width:0] l_r;
   reg [s_width:0] s_r;
   always @(posedge clk) begin
    if(rst) begin
        l_r <= 'b0;
        s_r <= 'b0;
    end
    else case(state)
        s10: begin
            l_r <= {1'b0,pow}+{2'b0,pow[s_width-1:1]};
            s_r <= {1'b0,pow};
        end
    endcase
   end
   
   //compare  s12 13
   reg  cmp_l_r;
   reg  cmp_s_r;
   reg  find_sp_r;
   assign  find_sp = find_sp_r;
   always @(posedge clk) begin
        if(rst) begin
            cmp_l_r <= 1'b0;
            cmp_s_r <= 1'b1;
        end
        else case(state)
            s12:begin
                cmp_l_r <= (cor_r < l_r)?1'b1:1'b0;
                cmp_s_r <= (cor_r > s_r)?1'b1:1'b0;
            end
            s13:begin
                find_sp_r <= cmp_l_r & cmp_s_r;
            end
        endcase
   end

   //arctan s10-s28
   parameter cordic_width = 80;
   reg  [cordic_width-1:0] cordic_in_r;
   reg                     cordic_en_r;
   reg  [width-1:0]        cordic_out_r;
   wire [cordic_width-1:0] cordic_in;
   wire                    cordic_en;
   wire                    cordic_fin;
   wire [width-1:0]        cordic_out; 
   assign cordic_en = cordic_en_r;
   assign cordic_in = cordic_in_r;
   cordic_ang cor_ins(clk, cordic_en, 
         cordic_in, cordic_fin, cordic_out);
   always @(posedge clk) begin
        if(rst) begin
            cordic_in_r<='b0;
            cordic_en_r<=1'b0;
        end
        else case(state)
            s10:begin
                cordic_in_r  <= {{4'b0000},$unsigned(sum_i),{4'b0000},$unsigned(sum_r)};
                cordic_en_r  <= 1'b1;
            end
            s28:begin
                cordic_out_r <= cordic_out;
                cordic_en_r  <= 1'b0;
            end
        endcase
   end
   
   assign cfo = cordic_out_r;



   
   /*
   //Wait
   reg [6:0] cnt;
   always @(posedge clk) begin
        if(rst) cnt<='b0;
        else case(state)
            s29:begin
                cnt<=cnt+1'b1;
            end
            default:cnt<='b0;           
        endcase
   end
   */
   
   //state machine
   always @(posedge clk) begin
        if(rst) state<=idle;
        else case(state)
            idle: state <= s0;
            s0:  state<= state<<1;
            s1:  state<= state<<1;
            s2:  state<= state<<1;
            s3:  state<= state<<1;
            s4:  state<= state<<1;
            s5:  state<= state<<1;
            s6:  state<= state<<1;
            s7:  state<= state<<1;
            s8:  state<= state<<1;
            s9:  state<= state<<1;
            s10:  state<= state<<1;
            s11:  state<= state<<1;
            s12:  state<= state<<1;
            s13:  state<= state<<1;
            s14:  state<= state<<1;
            s15:  state<= state<<1;
            s16:  state<= state<<1;
            s17:  state<= state<<1;
            s18:  state<= state<<1;
            s19:  state<= state<<1;
            s20:  state<= state<<1;
            s21:  state<= state<<1;
            s22:  state<= state<<1;
            s23:  state<= state<<1;
            s24:  state<= state<<1;
            s25:  state<= state<<1;
            s26:  state<= state<<1;
            s27:  state<= state<<1;
            s28:  state<= state<<1;
            //s29:  state <= (cnt==7'd66)?s0:s29;
            s29: state<= (strb )?s0:s29;
            default:state<= idle;
        endcase
   end
endmodule